/**
 * @file        star_controller.cpp
 * @authors     北京工业大学 PIP 战队 24 龚政
 * @version     v1.0
 * @details
 *      同时控制turtlesim的两只乌龟，以不同方式，按照一定顺序遍历到达0，1，2，3，4共五个点位并循环
 * 两只乌龟协助绘制五角星及其外接正五边形
 * 乌龟轨迹颜色可以R、G、B顺序循环变换
 */

#include <cmath>  // 数学库，用于计算角度和距离

#include "geometry_msgs/msg/twist.hpp"  // 几何消息类型，用于控制速度
#include "rclcpp/rclcpp.hpp"            // ROS 2 核心头文件
#include "turtlesim/msg/pose.hpp"       // 乌龟的位置姿态消息
#include "turtlesim/srv/set_pen.hpp"    // 设置乌龟颜色的服务
#include "turtlesim/srv/spawn.hpp"      // 生成新乌龟的服务

#define PI 3.14159

class StarController : public rclcpp::Node {
 public:
  StarController()
      : Node("star_controller"),
        state1_(State::FORWARD),
        state2_(State::TURN_RIGHT),
        distance_traveled1_(0.0),
        angle_turned1_(0.0),
        distance_traveled2_(0.0),
        angle_turned2_(0.0),
        color_index_(0),
        turtle1_pose_received_(false) {
    // 初始化第一只乌龟的控制
    velocity_publisher1_ =
      this->create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);
    pose_subscriber1_ = this->create_subscription<turtlesim::msg::Pose>(
      "turtle1/pose", 10,
      std::bind(&StarController::poseCallback1, this, std::placeholders::_1));

    // 初始化第二只乌龟的控制
    velocity_publisher2_ =
      this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 10);
    pose_subscriber2_ = this->create_subscription<turtlesim::msg::Pose>(
      "turtle2/pose", 10,
      std::bind(&StarController::poseCallback2, this, std::placeholders::_1));

    // 创建定时器，用于控制两个乌龟的移动
    timer_ =
      this->create_wall_timer(std::chrono::milliseconds(100),
                              std::bind(&StarController::timerCallback, this));
  }

 private:
  enum class State { FORWARD, TURN, TURN_RIGHT, TURN_LEFT };

  void poseCallback1(const turtlesim::msg::Pose::SharedPtr msg) {
    if (!turtle1_pose_received_) {
      // 获取第一只乌龟的初始坐标
      initial_pose1_ = *msg;
      turtle1_pose_received_ = true;

      // 设置第一只乌龟的轨迹颜色为红
      setPenColor("turtle1", 255, 0, 0);

      // 启动第二只乌龟
      spawnTurtle2();
    }
    current_pose1_ = *msg;
  }

  void poseCallback2(const turtlesim::msg::Pose::SharedPtr msg) {
    current_pose2_ = *msg;
  }

  void timerCallback() {
    if (!turtle1_pose_received_) {
      // 等待获取第一只乌龟的初始位姿
      return;
    }

    // 控制第一只乌龟
    auto twist_msg1 = geometry_msgs::msg::Twist();
    if (state1_ == State::FORWARD) {
      twist_msg1.linear.x = 1.0;
      distance_traveled1_ += 0.1;
      if (distance_traveled1_ >= 3.0) {
        state1_ = State::TURN;
        distance_traveled1_ = 0.0;
      }
    } else if (state1_ == State::TURN) {
      twist_msg1.angular.z = 0.5;
      angle_turned1_ += 0.05;
      if (angle_turned1_ >= PI * 4 / 5 - 0.02) {
        state1_ = State::FORWARD;
        angle_turned1_ = 0.0;
        updatePenColor("turtle1");
      }
    }
    velocity_publisher1_->publish(twist_msg1);

    // 控制第二只乌龟
    auto twist_msg2 = geometry_msgs::msg::Twist();
    if (state2_ == State::TURN_RIGHT) {
      twist_msg2.angular.z = -0.5;
      angle_turned2_ += 0.05;
      if (angle_turned2_ >= PI / 5 - 0.05) {
        state2_ = State::FORWARD;
        angle_turned2_ = 0.0;
      }
    } else if (state2_ == State::FORWARD) {
      twist_msg2.linear.x = 0.5;
      distance_traveled2_ += 0.05;
      if (distance_traveled2_ >= 1.86) {
        state2_ = State::TURN_LEFT;
        distance_traveled2_ = 0.0;
      }
    } else if (state2_ == State::TURN_LEFT) {
      twist_msg2.angular.z = 0.5;
      angle_turned2_ += 0.05;
      if (angle_turned2_ >= PI * 2 / 5 - 0.05) {
        state2_ = State::FORWARD;
        angle_turned2_ = 0.0;
      }
    }
    velocity_publisher2_->publish(twist_msg2);
  }

  void spawnTurtle2() {
    // 创建一个临时节点
    auto temp_node = rclcpp::Node::make_shared("spawn_client");

    // 创建服务客户端
    auto client = temp_node->create_client<turtlesim::srv::Spawn>("spawn");

    while (!client->wait_for_service(std::chrono::seconds(1))) {
      RCLCPP_WARN(temp_node->get_logger(), "Waiting for /spawn service...");
    }

    auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
    request->x = initial_pose1_.x;  // 设置初始位置为第一只乌龟的坐标
    request->y = initial_pose1_.y;
    request->theta = initial_pose1_.theta;  // 设置初始方向
    request->name = "turtle2";

    auto result = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(temp_node, result) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(temp_node->get_logger(), "Failed to spawn turtle2");
    }
  }

  void setPenColor(const std::string &turtle_name, int r, int g, int b) {
    // 创建一个临时节点
    auto temp_node = rclcpp::Node::make_shared("set_pen_client");

    auto client = temp_node->create_client<turtlesim::srv::SetPen>(turtle_name +
                                                                   "/set_pen");
    if (!client->wait_for_service(std::chrono::seconds(1))) {
      RCLCPP_WARN(temp_node->get_logger(),
                  "Service %s/set_pen not available, skipping color change",
                  turtle_name.c_str());
      return;
    }

    auto request = std::make_shared<turtlesim::srv::SetPen::Request>();
    request->r = r;
    request->g = g;
    request->b = b;
    request->width = 3;
    request->off = 0;

    auto result = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(temp_node, result) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(temp_node->get_logger(), "Failed to set pen color for %s",
                   turtle_name.c_str());
    }
  }

  void updatePenColor(const std::string &turtle_name) {
    switch (color_index_) {
      case 0:
        setPenColor(turtle_name, 0, 255, 0);
        break;  // 绿
      case 1:
        setPenColor(turtle_name, 0, 0, 255);
        break;  // 蓝
      case 2:
        setPenColor(turtle_name, 255, 0, 0);
        break;  // 红
    }
    color_index_ = (color_index_ + 1) % 3;
  }

  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher1_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher2_;
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscriber1_;
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscriber2_;

  rclcpp::TimerBase::SharedPtr timer_;

  turtlesim::msg::Pose current_pose1_, initial_pose1_;
  turtlesim::msg::Pose current_pose2_;

  State state1_, state2_;
  double distance_traveled1_, angle_turned1_;
  double distance_traveled2_, angle_turned2_;
  int color_index_;
  bool turtle1_pose_received_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StarController>());
  rclcpp::shutdown();
  return 0;
}
